import logging
import unittest

from PathGenerator import *

class CreatePathGenerator(unittest.TestCase):
    
    def setUp(self):
        pass

    def testPath(self):
        g = PathGenerator()
        s = State()
        s.arm1angle = 0
        e = Event(s, 0.0)
        sGoal = State()
        sGoal.arm1angle = 1.0
        eGoal = Event(sGoal, 1.0)
        g.generatePath(e, eGoal)
        for t in range(0, 28):
            s = g.getState(t/10.0)
            #print t/10.0, s.arm1angle, s.arm1angularVelocity

    def testPathStop(self):
        g = PathGenerator()
        s = State()
        s.arm1angle = 0
        s.arm1angularVelocity = 0.3
        e = Event(s, 0.0)
        sGoal = State()
        sGoal.arm1angle = 1.0
        eGoal = Event(sGoal, 1.0)
        g.generatePath(e, eGoal)
        for t in range(0, 28):
            s = g.getState(t/10.0)
            #if s != None:
            #    print t/10.0, s.arm1angle, s.arm1angularVelocity

#logging.basicConfig(level=logging.DEBUG, format='%(asctime)s %(levelname)s: %(message)s', datefmt='%H:%M:%S')

if __name__ == "__main__":
    unittest.main()   